# -- coding:utf-8 --
#!/usr/bin/python
import sys, os
import inspect
from SnakeRobot import SnakeRobot
from BwRobotLib import BwRobotLib
from MiniBotControlLib import MiniBotControlLib
import time
import BwHelpLib 
import _thread

# 加载机器人构型配置
fileName = inspect.getframeinfo(inspect.currentframe()).filename
folderPath = os.path.dirname(os.path.abspath(fileName))
config_file = folderPath+"/config.txt"

class SnakeRobotClass:
    def __init__(self):
        # 实例化MiniBotControlLib类，并设置为udp模式
        self.mlib = MiniBotControlLib("udp")
        # 输入ip地址和端口号
        self.ip = "192.168.1.168"
        self.port = 50000
        # 连接机器人,并设置超时时间2s
        self.mlib.Connect(self.ip, self.port, 2000)   
        # 实例化BwRobotLib类，并将mlib作为参数传入
        self.robotlib = BwRobotLib(self.mlib)
        # 导入配置文件，获取文件中的模块ID和模块连接位置
        self.module_ids, self.module_ids_loc = self.robotlib.loadConfig(config_file)
        # 扫描当前机器人构型的ID，默认扫描ID段为20-50
        self.module_ids_scan, self.module_ids_loc_scan = self.mlib.ScanId()
        # 判断是否扫描的ID与配置文件导入的ID一致
        res = BwHelpLib.checkListEqual(self.module_ids,self.module_ids_scan)
        if res == False:
            print("\033[31m输入的模块ID与扫描获取的模块ID不一致，请检查！\033[37m")
            print("配置文件中模块ID: ", self.module_ids)
            print("实际扫描的模块ID: ", self.module_ids_scan)
            exit(0)
        else:
            print("输入的模块ID与扫描获取的模块ID一致")
            
        # 实例化操作臂类
        self.snakeRobot = SnakeRobot(self.mlib)
        # 将模块ID与操作臂构型绑定
        self.snakeRobot.bindIds(self.module_ids)        

    #运行线程
    def snakeRun(self,threadName,arg):
        self.snakeRobot.run()

    #运行
    def runDemo(self):
      waitSeconds = 2
      self.snakeRobot.goForward()
      time.sleep(waitSeconds)
      self.snakeRobot.goBack()
      time.sleep(waitSeconds)
      self.snakeRobot.turnLeft()
      time.sleep(1)
      self.snakeRobot.turnRight()
      time.sleep(1)
      self.snakeRobot.stop()

def main():
    # print("输入任意字符 Enter 开始初始化...")
    snake_robot = SnakeRobotClass()
    snake_robot.snakeRobot.stop()
    _thread.start_new_thread(snake_robot.snakeRun,("runthread",snake_robot.snakeRobot))
    while(1):
      snake_robot.runDemo()
if __name__ == "__main__":
    try :
        main()    
    except KeyboardInterrupt:
        print(" Program closed! ")
    pass

